#include <stdio.h>
#include <windows.h>

#include <math.h>


// 电机参数
const float P = 5; // 极对数
const float lambda_m = 0.0016; // 永磁体磁链
const float L_d = 0.000035; // d轴电感
const float L_q = 0.00005; // q轴电感


const float TOLERANCE = 1e-6; // 迭代停止的误差容忍度
const int MAX_ITER = 1000; // 最大迭代次数


// 电流限制
const float Imax = 700.0; // 最大电流限制



// 电机参数（根据电机规格设置）
// const float lambda_m = 0.1;
// const float L_d = 0.01;
// const float L_q = 0.015;
// const int P = 4;  // 极对数
// const float TOLERANCE = 1e-6; // 迭代停止的误差容忍度
// const int MAX_ITER = 1000; // 最大迭代次数

// 计算转矩的函数
float calculate_torque(float id, float iq) {
    return 1.5 * P * (lambda_m * iq + (L_d - L_q) * id * iq);
}

// 数值求解法
void mtpa_numerical(float torque_demand, float *id_ref, float *iq_ref) {
    float id = 0;
    float iq = sqrt(2 * torque_demand / (3 * P * lambda_m));
    float error = calculate_torque(id, iq) - torque_demand;
    int iter = 0;

    while (fabs(error) > TOLERANCE && iter < MAX_ITER) {
        id -= 0.01; // 调整步长
        iq = sqrt(2 * torque_demand / (3 * P * lambda_m) - id * id);
        error = calculate_torque(id, iq) - torque_demand;
        iter++;
    }

    if (iter >= MAX_ITER) {
        printf("数值求解未收敛\n");
    }

    *id_ref = id;
    *iq_ref = iq;
}

// 几何解析法
void mtpa_geometric(float torque_demand, float *id_ref, float *iq_ref) {
    float id, iq;
    float constant = lambda_m / (L_d - L_q);
    float torque_term = (2 * torque_demand) / (3 * P * (L_d - L_q));
    
    id = -constant / 2;
    iq = sqrt(pow(torque_term, 2) - pow(id, 2));

    *id_ref = id;
    *iq_ref = iq;
}

// 解析法
void mtpa_analytical(float torque_demand, float *id_ref, float *iq_ref) {
    float id, iq;

    id = -lambda_m / (2 * (L_d - L_q));

    float term = pow((2 * torque_demand) / (3 * P * (L_d - L_q)), 2) - pow(id, 2);

    if (term >= 0) {
        iq = sqrt(term);
    } else {
        iq = 0;
        id = 0;
    }

    *id_ref = id;
    *iq_ref = iq;
}



void mtpa_newton_raphson(float torque_demand, float current_magnitude, float *id_ref, float *iq_ref) {
    float id = 0.0, iq = current_magnitude; // 初始猜测
    float f1, f2;
    float J11, J12, J21, J22;
    float det, delta_id, delta_iq;
    const float tolerance = 1e-6;
    int max_iterations = 100;
    int iteration = 0;

    while (iteration < max_iterations) {
        // 计算函数值
        f1 = (3.0 / 2.0) * P * (lambda_m * iq + (L_d - L_q) * id * iq) - torque_demand;
        f2 = id * id + iq * iq - current_magnitude * current_magnitude;

        // 检查收敛性
        if (fabs(f1) < tolerance && fabs(f2) < tolerance) {
            break;
        }

        // 计算雅可比矩阵
        J11 = (3.0 / 2.0) * P * (L_d - L_q) * iq;
        J12 = (3.0 / 2.0) * P * (lambda_m + (L_d - L_q) * id);
        J21 = 2 * id;
        J22 = 2 * iq;

        // 计算雅可比矩阵的行列式
        det = J11 * J22 - J12 * J21;
        if (fabs(det) < tolerance) {
            printf("Warning: Jacobian determinant is close to zero.\n");
            break;
        }

        // 计算增量
        delta_id = (f1 * J22 - f2 * J12) / det;
        delta_iq = (f2 * J11 - f1 * J21) / det;

        // 更新值
        id -= delta_id;
        iq -= delta_iq;

        iteration++;
    }

    // 返回计算结果
    *id_ref = id;
    *iq_ref = iq;
}





// 定义电机参数
#define Ld 0.000035f  // d轴电感
#define Lq 0.00005f // q轴电感
#define psi_f 0.0016f  // 磁链常数
#define Is 110.0f   // 总电流

// 定义目标函数，即转矩公式
float torque_function(float Id, float Iq) {
    float P = 5.0f;  // 假设电机是两极电机
    float Te = 1.5f * P * (psi_f * Iq + (Ld - Lq) * Id * Iq);
    return Te;
}

// 简单的数值优化方法
void optimize_MTPA() {
    float Id = 0.0f, Iq = 0.0f; // 初始电流值
    float step = 0.1f;  // 迭代步长
    float Te_max = -1e9f; // 初始最大转矩设为一个很小的负数
    float Te_current;
    float best_Id = 0.0f, best_Iq = 0.0f;

    // 简单的迭代过程，可以根据实际情况改进为更复杂的优化算法
    for (Id = 0.0f; Id <= Is; Id += step) {
        for (Iq = 0.0f; Iq <= Is; Iq += step) {
            // 检查总电流约束
            if (sqrtf(Id * Id + Iq * Iq) <= Is) {
                Te_current = torque_function(Id, Iq);
                if (Te_current > Te_max) {
                    Te_max = Te_current;
                    best_Id = Id;
                    best_Iq = Iq;
                    // 打印当前最优解（可选）
                    printf("当前最优解：Id = %.2f A, Iq = %.2f A, Te = %.2f Nm\n", Id, Iq, Te_max);
                }
            }
        }
    }

    // 输出最终结果
    printf("\n最大转矩电流比控制下的最优解：\n");
    printf("Id = %.2f A\n", best_Id);
    printf("Iq = %.2f A\n", best_Iq);
    printf("最大转矩 Te = %.2f Nm\n", Te_max);
}


// 其他变量
float id = 0.0f;  // d 轴电流
float iq = 0.0f;  // q 轴电流
// MTPA 控制函数
void MTPA_control(float te_ref) {
    // 计算 MTPA 角度
    float theta_mtpa = atan2f(0.00005 * id, 0.000035 * iq);

    // 根据 MTPA 角度调节定子电流
    id = te_ref / (1.5f * 0.0167 * sinf(theta_mtpa));
    iq = id * tanf(theta_mtpa);
}


// 测试函数
int main() {
   //optimize_MTPA(); // 调用优化函数
//  
   
    float torque_demand = 1.0;  // 期望转矩
    float current_magnitude = 300.0; // 电流幅值
    float id_ref, iq_ref;

    mtpa_newton_raphson(torque_demand, current_magnitude, &id_ref, &iq_ref);

    printf("MTPA (Newton-Raphson) -> i_d: %f, i_q: %f\n", id_ref, iq_ref);


    system("pause");
    return 0;
}